#include<focctrl.h>


void FocCtrl::Init(SERIAL_CLASS* _CtrlSer)
{
    CtrlSer = _CtrlSer;
}


void FocCtrl::RxFocMotorInfo() 
{
    // Check for new data availability in the Serial buffer
  if (CtrlSer->available()) {
      incomingByte 	  = CtrlSer->read();                                   // Read the incoming byte
      bufStartFrame	= ((uint16_t)(incomingByte) << 8) | incomingBytePrev;       // Construct the start frame
  }
  else {
      return;
  }

  // Copy received data
  if (bufStartFrame == START_FRAME) {	                    // Initialize if new data is detected
      p       = (byte *)&Feedback;
      *p++    = incomingBytePrev;
      *p++    = incomingByte;
      idx     = 2;	
  } else if (idx >= 2 && idx < sizeof(MotorFeedback)) {  // Save the new received data
      *p++    = incomingByte; 
      idx++;
  }	

  // Check if we reached the end of the package
  if (idx == sizeof(MotorFeedback)) {
      uint16_t checksum;
      checksum = (uint16_t)(Feedback.start ^ Feedback.rpmR ^ Feedback.rpmL
                            ^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.curL_DC 
                            ^ Feedback.curR_DC);

      // Check validity of the new data
      if (Feedback.start == START_FRAME && checksum == Feedback.checksum) {   
          //Print data to built-in Serial
        //   Serial.print("R1: ");   Serial.print(Feedback.rpmR);
        //   Serial.print(" R2: ");  Serial.print(Feedback.rpmL);
        //   Serial.print(" R3: ");  Serial.print(Feedback.boardTemp);
        //   Serial.print(" R4: ");  Serial.print(Feedback.curL_DC);
        //   Serial.print(" R5: ");  Serial.print(Feedback.batVoltage);
        //   Serial.print("\r\n");
        rpm.rightRpm = Feedback.rpmR;
        rpm.leftRpm = Feedback.rpmL;
      } else {
        Serial.println("Non-valid data skipped");
      }
      idx = 0;    // Reset the index (it prevents to enter in this if condition in the next cycle)
  }

    // Update previous states
  incomingBytePrev = incomingByte;
}


void FocCtrl::MotorSendData(int16_t uRight, int16_t uLeft)
{
        Command.start   	= (uint16_t)START_FRAME;
		Command.mSpeedR   = (int16_t)uRight;
		Command.mSpeedL   = (int16_t)uLeft;
		Command.checksum 	= (uint16_t)(Command.start ^ Command.mSpeedR ^ Command.mSpeedL);

        CtrlSer->write((uint8_t *) &Command, sizeof(Command)); 
}


FocCtrl::MotorRpm  FocCtrl::GetMotorRpm()
{
    return rpm;
}